#include "message.h"
#include "command.h"
#include "serial.h"
#include "pc_message.h"
#include "bsp/cpu_driver.h"
#include "utils/logger.h"
#include "utils/utils.h"
#include "os/os.h"
#include "led.h"

static void message_call_back(head_t id, uint8_t *data, int len, u8 from);
static void free_message(message_t *message);
static void message_process(message_t *message);
#define MAX_CAN_MESSAGE 10
static message_t messages[MAX_CAN_MESSAGE];
static led_t led_blue;
static led_t led_green;

static serial_t m_serial;

void handle_serial_frame(uint8_t *data, int len) {
	head_t can_id;
	u8 flags = data[0];
    data += 1;
    len  -= 1;
	memcpy(&can_id, data, 4);
	data += 4;
	len  -= 4;
	if (can_id.dest != MY_MESSAGE_ADDR) {
		if (len <= 8) {
			can_route_message(can_id.id, data, len);
		}
	}else {
		message_call_back(can_id, data, len, MSG_DEV_SEIRAL);
	}
	led_on(&led_blue);
}


void handle_can_frame(u32 id, u8 *data, int len) {
	head_t can_id;
	can_id.id = id;
	if (can_id.dest == MY_MESSAGE_ADDR) {
		message_call_back(can_id, data, len, MSG_DEV_CAN);
	}else {
		serial_route_message(id, data, len);
	}
	led_on(&led_green);
}

void message_init(void) {

	led_init(&led_blue, led_blue_on);
	led_init(&led_green, led_green_on);
    m_serial.hw_id = CONFIG_HW_ID_UART0;
    m_serial.rate = 500000;
    m_serial.rx_handler = handle_serial_frame;
    serial_init(&m_serial);

	can_drv_init(handle_can_frame);

}

static message_t *get_message_by_id(head_t id, u8 hw){
	message_t *idle_msg = NULL;
	message_t *src_msg = NULL;
	for (int i = 0; i < MAX_CAN_MESSAGE; i++){
		/*first found the same src&dest */
		if ((messages[i].src == id.src) && (messages[i].dest == id.dest) && (messages[i].from == hw)){
			return (messages + i);
		}
		if ((messages[i].src == id.src) && (messages[i].from == hw)){
			src_msg = messages + i;
		}else if (messages[i].data == NULL){
			idle_msg = messages + i;
		}
	}
	return ((src_msg!=NULL)?src_msg:idle_msg);
}

int serial_route_message(u32 efid, u8 *buff, int len) {
	return serial_route_frame(&m_serial, efid, buff, len);
}

int serial_send_message(u32 efid, u8 *buff, int len){

	serial_t *serial = &m_serial;
	head_t frame_id;
	u32 total_frames = ((len + 7) >> 3);
	int send_len = len;
	u32 id = 1;

	frame_id.id = efid;
	frame_id.total = total_frames;

	while(send_len > 0){
		frame_id.idx = id;
		int count = _min(8,send_len);
		frame_id.length = count;
		serial_send_frame(serial, frame_id.id, buff + (len - send_len), count);
		send_len -= count;
		id ++;
	}
	return len;
}


static void message_call_back(head_t id, uint8_t *data, int len, u8 from){
	sys_debug("id = %0x, data = ", id.id);
	for(int i = 0; i < len; i++) {
		sys_debug("0x%x ", data[i]);
	}
	sys_debug("\n");
	message_t *message = get_message_by_id(id, from);
	if (message == NULL) {
		return ;
	}
	uint16_t key = 0;
	message->from = from;
	if (len > 8) {
		key = decoder_key(data);
		len -= 2;
		message->key = key;
		message->dest = id.dest;
		message->src = id.src;
		message->type= id.type;
		message->total_frame = 1;
		message->idx = 1;
		message->len = len;
		if (len > 0) {
			message->data = os_alloc(len);
			memcpy(message->data, data+2, len);
		}else {
			message->data = NULL;
		}
		message_process(message);
		return;
	}
	int total = id.total?id.total:32;
	int idx = id.idx?id.idx:32;

	if((total <= MESSAGE_MAX_FRAMES) && (idx <= MESSAGE_MAX_FRAMES)
			&& (idx <= total)){
		if ((idx == 1) && (len >= 2)) { //first frame for message
			key = decoder_key(data);
			if (message->data) {
				os_free(message->data);
			}

			message->key = key;
			message->dest = id.dest;
			message->src = id.src;
			message->data = os_alloc(total * 8);
			message->len = 0;
			message->idx = idx;
			message->type= id.type;
			message->total_frame = total;
			len = len - 2; //skip key
			data = data + 2;
		}
		if (message->data){
			if ((message->idx == idx) && (message->total_frame == total)){
				memcpy(message->data + message->len, data, len);
				message->len += len;
				if (idx == total) { //last frame
					message_process(message);
				}
				message->idx = (idx + 1);
			}else {
				free_message(message);
			}
		}
	}
}

static void message_process(message_t *message){
	//sys_debug("can %x [%x -> %x], len = %d\n", message->key, message->src, message->dest, message->len);
	if (message->dest != MY_MESSAGE_ADDR && message->from == MSG_DEV_SEIRAL) {
		u32 id = get_can_id(message->dest, message->src, message->type, 0);
		can_send_message(id, message->data, message->len);
	}else if ((message->key & 0xFF) >= 0xF0) {
		process_iap_message(message);
	}
	if (message->data) {
		os_free(message->data);
	}
	free_message(message);
}

static void free_message(message_t *message){
	message->data = NULL;
	message->src = 0;
	message->dest = 0;
	message->len = 0;
	message->idx = 0xFF;
	message->total_frame = 0xFF;
	message->from = 0;
}
